﻿
#include <iostream>

#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "RCalibrate.h"
#include "RSolvePnp.h"


using namespace cv;
using namespace std;
using namespace RobotVision;
vector<String> ReadFileImage(string path)
{
	vector<String> files;
	glob(path, files, true);
	return files;
}

void main()
{
	//String FileRoad = "F:\\1 华南理工大学项目代码\\Halcon\\图片\\标定图片\\新建文件夹\\*.jpg";
	//String FileRoad = "F:\\1 华南理工大学项目代码\\CPP\\cv-tool\\image\\*.jpg";
	String FileRoad = "F:\\1 华南理工大学项目代码\\MyImage\\机器人标定图\\棋盘格\\5\\*.bmp";
	//String FileRoad = "F:\\1 华南理工大学项目代码\\CPP\\cv-tool\\image\\*.jpg";

	vector<Mat> images;
	vector<String> files = ReadFileImage(FileRoad);
	for (int indnex = 0; indnex < files.size(); indnex++)
	{
		images.push_back(imread(files[indnex],cv::IMREAD_GRAYSCALE));
	}

	//求解内参数
	RCalibrate calibrate = RCalibrate();
	RCalibrateParam calibrateParam = RCalibrateParam(images,Size(6,8),Size(25,25));
	//RCalibrateParam calibrateParam = RCalibrateParam(images, Size(6, 9), Size(1, 1));
	calibrate.Initialize(calibrateParam);
	calibrate.Run();
	RCalibrateResult calibrateResult= calibrate.GetResult();

	//求解PnP
	RSolvePnp solvePnp = RSolvePnp();
	RSolvePnpParam solvePnpParam = RSolvePnpParam(calibrateResult.ObjectPoints[0], calibrateResult.ImagePointSeq[0], calibrateResult.CameraMatrix, calibrateResult.DistCoeffs);
	solvePnp.Initialize(&solvePnpParam);
	solvePnp.Run();
	RSolvePnpResult a=solvePnp.GetResult();
	cout << "pnp求解结果:"<<endl << a.Rvec << endl << a.Tvec << endl;
	cout<<"标定求解结果" << endl << calibrateResult.RvecsMat[0] << endl << calibrateResult.TvecsMat[0] << endl;
	
}




